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Nonlinear  Symplectic  Attitude  Estimation  for  Small 

Satellites 


James  M.  Valpiani**and  Philip  L.  Palmer* 

Surrey  Space  Centre ,  Guildford ,  Surrey,  GU2  7XH,  England 


A  novel  method  for  efficient  high-accuracy  satellite  attitude  estimation  is  presented  to 
address  the  increasing  performance  requirements  of  resource-constrained  small  satellites. 
Symplectic  numerical  methods  are  applied  to  the  nonlinear  estimation  problem  for  Hamil¬ 
tonian  systems,  leading  to  a  new  general  solution  that  exactly  preserves  state  probability 
density  functions  and  conserves  invariant  properties  of  the  dynamics  when  solving  for  the 
state  estimate.  This  nonlinear  Symplectic  Filter  is  applied  to  a  standard  small  satellite 
mission  and  simulation  results  demonstrate  orders  of  magnitude  improvement  in  state  and 
constants  of  motion  estimation  when  compared  to  extended  and  iterative  Kalman  methods, 
particularly  in  the  presence  of  nonlinear  dynamics  and  high  accuracy  attitude  observations. 
Based  on  numerous  simulations,  the  authors  conclude  that  this  new  method  shows  promise 
for  improved  attitude  estimation  onboard  high  performance,  resource-constrained  small 
satellites. 


I.  Introduction 

ACCURATE  Attitude  Determination  and  Control  Subsystem  (ADCS)  algorithms  are  crucial  for  successful 
satellite  operations.  This  is  particularly  true  for  small  satellites,  where  maturing  customer  needs  and 
broadening  mission  profiles  have  led  to  increasingly  stringent  precision  and  performance  requirements.  Of 
paramount  concern  to  small  satellite  ADCS  engineering  is  the  ability  to  autonomously,  accurately,  and 
quickly  determine  orientation  in  space  with  minimal  expense  of  onboard  resources.  The  objective  of  this 
research  is  the  novel  application  of  symplectic  numerical  methods  to  small  satellite  attitude  estimation  in 
order  to  meet  these  requirements. 

In  1968,  Athans  et  al1  identified  two  main  methods  that  were  being  used  to  solve  the  nonlinear  filtering 
problem.  The  first  method  approximated  the  nonlinear  dynamical  and  observation  equations  in  order  to 
utilize  linear  filtering  theory.  The  second  method  used  approximations  to  the  exact  nonlinear  filtering 
solutions  such  as  those  found  in  References  2  and  3.  Broadly  speaking,  estimation  solutions  continue  to 
fall  into  one  of  these  two  categories.4-6  Primarily  because  of  its  low  computational  burden,  the  majority  of 
nonlinear  estimation  methods  used  on  satellites  have  fallen  into  the  former  category,  including  the  ubiquitous 
Extended  Kalman  Filter  (EKF). 2,7-9  While  this  approach  has  been  used  extensively,  it  renders  estimation 
methods  poorly-suited  for  highly  nonlinear  dynamical  systems,  leading  to  an  increased  demand  on  satellites 
to  continually  update  attitude  orientation  knowledge.10  On  the  other  hand,  approximating  optimal  nonlinear 
filtering  is  particularly  difficult  since  the  solution  to  one  of  the  two  governing  equations,  the  Fokker-Planck 
Equation  (also  known  as  the  Kolmogorov  Forward  Equation),  is  a  probability  density  function  extending  over 
an  infinite  domain.  Many  efforts  have  been  made  to  approximate  or  solve  numerically  for  exact  solutions; 
among  these  methods  are  Monte  Carlo  methods,  finite-difference  methods,  and  Fourier  series  representations, 
all  of  which  have  been  documented  as  computationally  expensive  or  difficult  to  implement  and  therefore 
unsuitable  for  online  implementation.11-13  The  qualities  of  both  these  nonlinear  estimation  methods  are 
troublesome  for  small  satellites  with  constrained  resources  and  ever-increasing  attitude  requirements.  Recent 
advancements  in  small  satellite  ADCS  capabilities  such  as  slewing  rates  of  3°s-1  or  better14  highlight  the 
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need  for  high  accuracy  efficient  attitude  estimation  methods  that  can  address  the  shortcomings  of  both  these 
approaches. 

In  parallel  to  these  considerations,  recent  research  in  the  field  of  estimation  has  focused  on  preserv¬ 
ing  the  symplectic  properties  of  Hamiltonian  systems.15-19  In  essence,  symplecticity  indicates  that  certain 
global  conservation  laws  apply  to  a  system  which  take  the  form  of  geometric  constraints  on  the  space  of 
possible  solutions.  While  the  concept  of  symplecticity  is  not  new,  numerical  techniques  that  preserve  these 
values  over  time  are.  Compared  to  other  methods,  symplectic-preserving  numerical  methods  have  demon¬ 
strated  increased  stability  and  improved  long-term  performance  when  applied  to  Hamiltonian  dynamical 
systems.  Consequently,  symplectic  methods  are  well  suited  for  accurate  long-term  propagation  and  systems 
where  preservation  of  conserved  quantities  is  important  for  accurate  predictions.  Recent  research  involv¬ 
ing  symplectic  numerical  methods  has  demonstrated  promising  results  when  applied  to  satellite  attitude 
propagation.10,20  Additionally,  symplectic  methods  have  led  to  significant  improvements  in  state  accuracy 
and  constants  of  motion  accuracy  when  applied  to  standard  EKF  theory  for  satellite  attitude  estimation 
(symplectic  EKF,  or  SKF17). 

This  paper  presents  a  novel  approach  for  addressing  the  shortcomings  of  the  two  methods  of  nonlinear 
filtering  described  above  by  exploiting  the  strength  of  the  symplectic  properties  of  satellite  motion.  Because 
the  dynamics  of  small  satellites  are  well  understood,  and  because  disturbances  act  on  long  time  scales  relative 
to  the  Hamiltonian  motion,  initial  theory  development  assumes  that  unmodeled  stochastic  accelerations 
between  measurement  updates  are  ignorable.  The  Fokker-Planck  equation  which  governs  the  evolution  of 
the  state  probability  density  function  (pdf)  is  then  reduced  to  a  deterministic  problem.  In  Hamiltonian 
systems,  this  leaves  the  probability  density  function  associated  with  the  system  state  invariant  over  time.21 
Since  the  resulting  deterministic  system  has  symplectic  geometry,  a  symplectic-preserving  integrator  is  used 
to  solve  for  the  evolution  of  the  pdf  exactly  with  the  additional  benefit  of  conserving  the  underlying  geometric 
structure  of  the  motion.  The  nonlinearly  propagated  pdf  is  then  combined  with  a  system  measurement  via 
Bayes’  Rule,  and  an  iterative  method  is  used  to  determine  the  state.  Once  the  state  has  been  solved  at 
an  update,  the  standard  Kalman  Filter  covariance  update  is  modified  for  Hamiltonian  systems  and  applied 
to  the  filter  in  order  to  create  a  computationally  efficient  algorithm.  Finally,  a  form  of  process  noise  is 
reintroduced  into  the  estimation  construct  in  order  to  increase  its  robustness  to  unmodeled  disturbances. 

In  this  paper,  a  new  nonlinear  solution  to  the  general  estimation  problem  for  Hamiltonian  systems 
is  presented.  First,  the  equations  of  motion  governing  satellite  attitude  are  defined,  followed  by  a  brief 
review  of  symplecticity  and  the  symplectic  attitude  propagator  used  for  this  research.  Then,  the  nonlinear 
Symplectic  Filter  (SF)  is  derived  and  applied  to  the  satellite  attitude  estimation  problem.  Comparisons  with 
standard  estimation  techniques  demonstrate  significantly  better  performance  particularly  in  the  presence  of 
high-accuracy  attitude  observations  and  nonlinear  dynamics.  Finally,  conclusions  are  drawn  based  on  these 
results. 


II.  Equations  of  Motion 

Fundamentally,  the  problem  of  three-axis  attitude  parameterization  is  to  specify  the  orientation  of  the 
satellite  body  frame  with  respect  to  another  known  coordinate  frame  (commonly,  the  local  orbital  or  inertial 
coordinate  frames).  The  equations  of  satellite  motion  which  govern  its  orientation  are  described  in  this 
section. 

Satellite  dynamics  in  inertial  space  are  governed  by  Euler’s  equations  of  motion.  Describing  the  rotational 
motion  of  the  satellite  with  respect  to  an  inertial  frame  gives 

h  =  hx(r‘h)  +  N  (1) 

where  h  =  Iu>b/i  is  the  body  angular  momentum  vector,  /  €  R3x3  is  the  moment  of  inertia  tensor  of  the 
satellite,  oJ^/i  =  [wi  W2  w3]r  is  the  inertially  referenced  body  angular  rate  vector,  N  £  l3  is  the  total 
external  torque  vector  about  the  satellite  center  of  mass  with  respect  to  the  inertial  coordinate  frame,  and 
subscripts  1,  2,  and  3  are  the  axes  of  the  satellite  body  coordinate  frame.  In  this  paper,  motion  of  the 
satellite  is  described  with  respect  to  principal  axes  so  that  the  off-diagonal  terms  of  I  become  zero  and  the 
diagonal  elements  are  referred  to  as  I\,  I2,  and  I3. 

The  orientation  of  the  satellite  body  between  the  inertial  frame  and  the  body-fixed  frame  can  be  repre¬ 
sented  by  Euler  parameters,  which  are  expressed  here  as  a  4- vector  quaternion  q  €  S3.  This  quaternion  is 
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governed  by22 
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The  most  significant  external  torque  affecting  small  satellites  is  gravity-gradient  torque, 
included  explicitly  in  the  dynamical  model23 


It  is  therefore 


^Gravity  Gradient 


3 fi  -r  -r 
X  It, — 17 


(5) 


where  ||  •  ||  is  the  Euclidean  norm,  /x  =  GM  is  the  Earth’s  gravitational  constant,  and  r  is  the  position  vector 
from  the  center  of  mass  of  the  Earth  to  the  satellite’s  center  of  mass  expressed  in  the  orbital  frame  (defined 
by  a  right-handed  orthogonal  set  of  axes  with  its  z-axis  in  the  nadir  direction  and  its  y-axis  in  the  orbit 
anti-normal  direction).  Note  that  these  equations  derive  from  a  noncanonical  Hamiltonian  system  that  is  a 
Lie-Poisson  system  with  skew-symmetric  structure  matrix  J(x):24 

|x  =  J(x)Vff(x)  (6) 

To  facilitate  later  discussion,  flow  map  notation  is  introduced  according  to  Ref.  21  to  describe  the  solution 
to  Eq.  (6).  Consider  the  satellite  equations  of  motion  written  in  general  terms 

jy-t  =  f{t,xt)  (7) 

with  initial  state  xo-  The  set  of  all  possible  solutions  defined  by  this  initial  state  forms  a  phase  space 
trajectory,  and  the  flow  map  4)  :  RJ  — >  RJ  defines  a  mapping  between  solutions  at  different  times  along  a 
given  trajectory.  The  flow  map  satisfies 

^  =  f(t,<t>(t\x0,to))  (8) 

0(to;xo,to)  =  Xo  (9) 


The  direct  flow  map  relates  an  initial  state  xq  at  time  to  to  some  future  state  xt  at  time  t: 


xt  =  4>(t;  x0,  to),  t  >  t0  (10) 

To  clarify  the  direction  of  time,  the  inverse  flow  map  is  defined  to  relate  an  initial  state  at  time  t  to  some 
previous  state  at  time  to 


x0  =  V’(to  ;xt,t)  (li) 

The  two  flows  are  related  by  the  identity 

xD  =  ■0(to;<A(^;xo,to),t)  (12) 

This  notation  will  be  used  in  Section  IV  to  explain  the  theoretical  basis  of  the  nonlinear  Symplectic  Filter. 
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III.  Symplecticity 


The  term  ‘symplectic’  is  associated  with  a  numerical  scheme  that  approximates  the  solution  to  a  Hamil¬ 
tonian  system  while  preserving  its  underlying  symplectic  structure.  More  specifically,  a  symplectic  integrator 
is  the  exact  solution  to  a  discrete  perturbed  Hamiltonian  system  that  is  close  to  the  actual  Hamiltonian  of 
interest.  Because  Hamiltonian  systems  have  volume-preserving  mappings  in  phase  space  (according  to  Li- 
ouville’s  Theorem),  symplectic  integrators  also  preserve  volume  in  phase  space  to  within  machine  precision. 
Therefore,  phase  space  trajectories  do  not  cross  and  energy  fluctuations  about  the  exact  energy  are  bounded 
in  solutions  given  by  these  methods.  Practically  speaking,  symplectic  integrators  preserve  the  invariants  of 
motion  for  Hamiltonian  systems  to  a  much  higher  degree  of  accuracy  than  non-symplectic  methods  of  the 
same  order.  Generally,  non-symplectic  methods  will  introduce  secular  errors  into  conserved  values  of  the 
dynamics  over  time  and  therefore  misrepresent  conservative  systems  as  a  dissipative  ones.25 

A.  Integrator 

The  symplectic  integrator  used  to  propagate  Eq.  (6)  for  this  research  is  found  in  Ref.  10.  Briefly,  it 
uses  the  second-order  implicit  midpoint  rule  to  integrate  Euler’s  moment  equations  in  conjunction  with  a 
second-order  leapfrog  scheme  that  governs  the  order  of  the  kinematics  propagation  with  respect  to  the  dy¬ 
namics  propagation.  This  composite  scheme  is  based  on  symmetric  methods,  making  it  conceptually  simple, 
stable,  and  straightforward  to  implement  and  to  increase  in  order.  Most  importantly,  it  is  computationally 
inexpensive  compared  to  other  published  symplectic  integrators.20,26 

The  integrator  solves  Eq.  (6)  with  a  few  minor  modifications.  In  keeping  with  standard  practice,  the 
propagated  quaternion  describes  the  rotation  between  the  body  frame  and  the  orbital  frame,  governed  by 

q=i[fi(0l)-fr(o2)]q  (13) 

where  Oi  =  \^J/i  oj  ,  o2  =  [0  n  0  0]T,  n  is  the  orbital  mean  motion,  and 

Pi  P3  ~P2  Pi  9i  94  ~93  92  9i  Pi 

fl  (p)  q  =  ~P3  Pi  Pl  P 2  92  =  n*(q)p  =  93  94  "9l  92  P2  (14) 

P2  -Pi  Pi  P3  93  -92  9l  94  93  P3 

.  “Pi  -P2  -P3  Pi  J  L  94  J  L  — 9l  -92  -  93  94  J  [  Pi  . 

The  difference  between  this  quaternion  and  the  body  to  inertial  frame  quaternion  described  in  Section  II  is 
a  known  rotation,  and  therefore  trivial. 

Internal  angular  momentum  changes  are  associated  with  active  control  being  applied  to  a  satellite,  and 
to  maintain  a  conservative  system  the  satellite  is  assumed  to  be  uncontrolled  over  the  course  of  an  individual 
propagation  step.  Similarly,  external  torques  are  assumed  to  be  zero  over  the  course  of  a  propagation  step, 
which  is  valid  given  that  the  primary  torque  (gravity  gradient)  is  modeled  and  that  other  torques  generally 
operate  on  timescales  much  longer  than  the  standard  ADCS  propagation  step.  The  leapfrog  structure  of  the 
integrator  makes  the  addition  of  internal  and  external  torques  as  impulsive  phase  space  trajectory  jumps  a 
straightforward  matter.26 

The  examples  in  this  paper  all  assume  circular  Keplerian  orbits  for  simplification,  as  this  eliminates  the 
need  to  propagate  the  translational  motion  variables  and  reduces  the  system  state  to  seven  variables  (four 
quaternions  and  three  angular  rates);  however,  the  estimation  method  presented  below  is  not  restricted 
to  this  assumption.  While  the  rigid  body  motion  first  integrals  of  rotational  energy  and  satellite  angular 
momentum  measured  in  the  inertial  frame  are  not  conserved  due  to  the  gravity  gradient  torque,  the  circular 
orbit  assumption  gives  rise  to  a  Jacobi-like  integral  constant10,20 

^-iX>»I  +  y^  +  5ppX>43(q)  (is) 

where  u>  =  u>b/i  and  L2  is  the  second  component  of  the  angular  momentum  vector  in  the  inertial  frame.  This 
vector  is  given  by 

L  =  D(n£)A(q)T(h)  (16) 
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where  D(nt)  £  50(3)  is  the  rotation  matrix  from  the  local  orbital  frame  to  the  inertial  frame  and  A(q)  £ 
50(3)  is  the  quaternion-based  rotation  matrix  from  the  local  orbital  frame  to  the  body  frame. 

In  flow  map  notation,  the  integrator  for  this  research  solves  Eq.  (6)  using  a  symplectic  map  <j>'  to 
approximate  the  true  direct  mapping  cp,  and  the  inverse  of  the  integrator,  ip',  approximates  the  true  inverse 
mapping  ip. 


IV.  The  Nonlinear  Symplectic  Filter 


A.  Theory  Development 

Fundamentally,  estimation  is  the  determination  of  a  system’s  state  in  the  presence  of  noisy  observations, 
uncertain  initial  conditions,  and  random  disturbances  too  complex  to  model.  This  research  effort  takes  the 
Bayesian  approach  in  solving  the  estimation  problem.  Consider  the  continuous  time  Ito  stochastic  differential 
equation2 

^xt  =  /(xt)  +  G(xt)r)t,  *  >  to  (17) 

where  xt  =  [x\  x2  ...  xm]J  £  Rm  is  the  system  state  at  time  t,  / (xt)  £  Rm  describes  the  deterministic 
behavior  of  the  state,  G(xt)  £  Rmxfc  characterizes  the  diffusion,  and  {rp  £  Rfc,  t  >  to}  >s  a  zero  mean  white 
Gaussian  noise  process  with  S{r]t,  rit'}  =  Q(t)5(t  —  £').  Observations  of  the  system  are  taken  at  discrete  time 
instants  t„, 


z„  =  h(xn)  +  vn;  n  =  1, 2, . . . ;  f0  <  tn  <  f„+i  (18) 

where  z„  £  Ir  is  a  vector  of  observations,  h(xn)  £  ®r  relates  the  system  states  to  the  observations,  and 
{vn  £  Rr,  n  =  1, . . .}  is  a  white  Gaussian  noise  process  with  v„  ~  N( 0,  Rn).  For  convenience,  the  state  x  at 
time  t  will  generally  be  written  as  xt  and  at  discrete  time  instants  tn  will  be  written  as  xn;  also,  functions  of 
the  state  are  generally  functions  of  time  t  (or  tn),  though  for  convenience  this  will  not  be  explicitly  stated. 
It  is  assumed  that  the  initial  state,  the  process  noise,  and  the  observation  noise  are  independent,  and  that 
some  initial  state  probability  density  p(xo)  is  known. 

The  nonlinear  estimation  problem  is  to  find  the  state  estimates  conditioned  on  the  observations.  This  is 
accomplished  by  determining  the  evolution  of  the  conditional  probability  density  function  p(xn\Zn),  where 
Zn  =  {zd  :  to  <  td  <  tn}  and  p(xo\Zo)  =  p(x o).  Knowing  the  conditional  density  function,  descriptive 
statistics  can  be  used  to  estimate  the  state.  Between  observations  at  times  tn_i  and  tn,  the  conditional  density 
p(xt|Zn_i)  satisfies  the  Fokker-Planck  equation  (also  known  as  the  Kolmogorov’s  Forward  equation)12 


d_ 

dt 


p—Z 


r )  .mm 


» 1 


2  Z—i  Z—i  dxidxi 

1=1  J  =  1  J 


P  [GQG1 


tn—  1  ^  t  tn 


(19) 


where  p(xt|Zn_i),  fi(xt),  G(xt),  and  Qt  were  replaced  by  p,  fi,  G,  and  Q,  respectively,  for  simplicity. 

Using  Eq.  (19),  some  conditional  density  p(xn_i|Zn_i)  can  be  evolved  in  time  up  to  an  observation  at 
time  fn,  giving  the  a  priori  density  p(xn|Z„_i).  Then,  the  a  posteriori  conditional  density  p(xn|Z„)  can  be 
determined  via  Bayes’  Rule2 

.  I V  1  p(zn |xn>  Zn-l)p{xn\Zn-l) 

P{xn\Zn)  —  /  i7  \  (20) 

p{Zn\^n—l) 

The  a  posteriori  conditional  density  p(xn| Zn)  is  the  complete  solution  of  the  nonlinear  estimation  problem 
because  it  embodies  all  statistical  information  about  x  at  tn  which  is  contained  in  the  observations  and  the 
initial  condition  p(xo).2 

Returning  to  the  system  dynamics  in  Eq.  (17),  note  that  Q  is  used  to  account  for  unknown  disturbances 
and  process  noise  by  modeling  them  as  stochastic.  However,  the  dynamics  for  small  satellites  are  well 
known10  and  for  the  time  being,  these  unknown  disturbances  are  considered  small  enough  to  be  ignored. 
Under  this  assumption,  the  Fokker-Planck  equation  reduces  to  a  deterministic  Hamiltonian  system,  as  shown 
below  (note  that  the  following  discussion  on  the  Fokker-Planck  equation  has  been  adapted  from  Scheeres 
and  Park21) 


d 


dt 


i=l 


dxi 


(21) 
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Using  the  Lie-Poisson  system  described  in  Section  II, 

^x  =  J(x)Vff(x) 


then  Eq.  (21)  can  be  written  as 


n  T 


dx 


P 


(22) 


Due  to  the  unique  structure  of  Eq.  (6)  which  is  a  result  of  the  system’s  Hamiltonian  nature,  it  can  be  shown 
that  Eq.  (22)  reduces  to 

dP  =  °  (23) 

Using  the  fundamental  theorem  of  calculus  and  the  integral  invariance  of  the  probability  of  a  dynamical 
system  without  diffusion,  it  can  be  shown  that  for  Hamiltonian  systems  Eq.  (23)  gives 

(24) 


p(<p(t;x.Q,t0))  =  p(x0) 


dxt 

5x0 


dx, 


9xo 


1  where 


is  the  determinant. 


The  symplectic  nature  of  the  Hamiltonian  system  implies  that27 
Therefore,  given  an  initial  state  and  its  associated  pdf,  the  flow  map  for  the  Hamiltonian  system  enables  the 
full  characterization  of  the  pdf  at  any  time: 


p(^(f;  x0,  t0))  =  p(xo)  (25) 

p(^(f0;x,t))  =  p(x0)  (26) 


For  the  satellite  attitude  problem,  the  true  solution  flow  <f>  is  nonlinear  and  requires  the  use  of  a  numerical 
integrator  in  order  to  solve  for  the  system  state.  While  symplectic  integrators  by  definition  satisfy 


dxt  _ 
dxo 


1,  the  most  commonly  applied  integrators  such  as  standard  Runge-Kutta  methods28  do  not  preserve  the 
symplectic  structure  of  Hamiltonian  systems  and  therefore  do  not  satisfy  the  flow  condition.  For  dynamical 
propagations  with  relatively  noisy  observations,  this  tends  not  to  be  a  significant  concern.  In  these  cases, 
the  observation  noise  drowns  out  any  differences  between  numerical  methods.  However,  small  satellites 
are  increasingly  using  high-accuracy  position  observations  in  lieu  of  rate-sensing  equipment,29  such  as  the 
ESA  PROBA-1  mission.  Its  star  camera,  the  Advanced  Stellar  Compass  (ASC),30  demonstrated  arc-second 
level  accuracy  and  performed  at  rates  of  up  to  2.5°s_1.  Recent  research  has  demonstrated  that,  given  this 
combination  of  nonlinear  dynamics  and  high  accuracy  observations,  the  use  of  a  symplectic  propagator  results 
in  significant  accuracy  improvements  over  standard  nonsymplectic  propagators  for  state  and  constants  of 
motion  estimation.17 

Considering  these  results  in  the  context  of  the  nonlinear  estimation  problem,  this  means  that  a  probability 
density  function  of  a  deterministic  Hamiltonian  system  can  be  completely  characterized  as  it  transforms 
forward  in  time  to  a  level  of  accuracy  limited  only  by  the  numerical  method  employed  for  state  propagation. 
Using  a  symplectic  method  to  propagate  the  density  function  for  the  satellite  attitude  problem  preserves 
the  integral  invariants  of  the  motion,  including  state  probability  and  the  Jacobi-like  constant  in  Eq.  (15). 
In  contrast,  approximate  nonlinear  estimation  techniques  only  propagate  a  finite  amount  of  information 
contained  in  the  density  function  (typically,  the  mean  and  covariance)  in  order  to  estimate  the  state.  Utilizing 
the  result  above,  all  information  contained  in  a  density  function  (generally  composed  of  an  infinite  number 
of  moments)  can  be  carried  forward  in  a  way  that  preserves  its  nonlinearity  and  the  underlying  geometric 
structure  of  the  Hamiltonian  system. 


B.  Symplectic  Filter  Equations 

1.  State  Estimation 

Applying  the  result  from  Subsection  A  to  the  estimation  problem,  assume  an  initial  conditional  density 
function  at  time  tn 

p(xn|Zn)  (27) 
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Equation  (25)  gives  the  evolution  of  this  initial  conditional  density  function  between  observations.  Therefore, 
at  the  next  observation  time  tn+\,  the  transformed  density  function  is 

P  (xn-)-i |  Zn)  =  p  [ip  (tn‘,  xn+i,  fn+i)|  Zn)  (28) 

Given  this  a  priori  density  function,  the  new  observation  zn+i,  and  its  density  function,  Bayes’  rule  can  be 
used  to  determine  the  a  posteriori  density  function  p  (xn+i  |  Zn+ 1). 

At  this  point,  no  assumptions  have  been  made  about  the  types  of  density  functions  used;  to  realize  an 
actual  filter,  these  functions  must  be  defined.  Assume  that  the  initial  state  density  p(xn\Zn )  is  Gaussian. 
Assume  also  that  observations  have  additive  white  Gaussian  noise  as  in  Eq.  (IS)  so  that  the  observation 
density  p(zn+i|xn+i,  Zn)  =  p(zn+i |xn+1).  Using  Bayes’  Rule  from  Eq.  (20) 

p  (xn_j_i \Zn+i)  =  ce“5()  (29) 

(')  —  [0  {pn\  *n+l )  tn+l)  ~~  Xn]  Pn  [0  {pn\ Xn+1,  tn+l)  ~  Xn]  +  [zn+l  ~  h  (xn+j)]  [zn_)_i  —  /l  (xn+j )] 

(30) 


where  xn  is  the  state  estimate  and  Pn  is  the  covariance  estimate  at  time  tn.  Note  that  the  constant 


f  P  (  zn+l  |  £1  tn+l  )  P  (£ i  tn+l  I  Zn)  d£ 

is  independent  of  x,  and  that  the  normalizing  constants - - — r  in  p  (zn+i  I  xn+i)  and - 4 — r  in  P  (x„ \Zn) 

l2nR„+,lt  |27rPn|5 

are  independent  of  x  and  therefore  cancel  between  the  numerator  and  denominator  when  applying  Eq.  (20). 

The  density  p(xn+1|Zn+i)  is  the  full  solution  to  the  nonlinear  estimation  problem,  and  its  mean  is 
commonly  used  as  the  state  estimate.  However,  because  of  the  constraint  imposed  by  the  nonlinear  inverse 
flow  map  in  Eq.  (29),  p(xn+i|  Zn+\)  is  generally  not  a  Gaussian  density  function  and  determining  its 
mean  is  nontrivial.  Most  methods  used  to  do  so  are  prohibitively  computationally  expensive  as  discussed 
in  Section  I.  If  p  (x„+i  |  Zn+\)  is  unimodal  and  concentrated  about  the  mean,  then  there  is  a  negligible 
difference  between  the  mean  and  the  mode  (or  peak)  of  the  density  function.4  In  fact,  Bell  and  Cathey 
proved  that  the  EKF  is  simply  the  first  Gauss-Newton  iterate  in  solving  for  the  mode  of  the  a  posteriori 
density  function,  and  that  the  Iterated  Extended  Kalman  Filter  (IEKF)  uses  multiple  Gauss-Newton  iterates 
to  solve  for  the  mode.2,4,31  In  addition  to  being  the  maximum  likelihood  estimator,  the  mode  represents  a 
possible  system  state  whereas  the  mean  is  not  always  guaranteed  to  do  so.  In  the  case  of  quaternions,  the 
means  of  each  of  the  quaternion  components  in  a  distribution  do  not  generally  compose  a  unit  quaternion.32 
For  these  reasons,  the  mode  is  used  as  the  state  estimate  for  this  filter. 

Determining  the  mode  of  the  a  posteriori  density  function  is  equivalent  to  finding  the  maximum  of  Eq. 
(29).  Since  the  maximum  of  p(x„+i|Zn+i)  is  the  maximum  of  any  monotonic  function  of  p  (xn+i  |  Zn+i), 
and  since  the  natural  logarithm  is  monotonic,  then  the  mode  of  the  pdf  can  be  stated  as4 


„  min 

x„+i  =  arg 

Xn-fl 


(■) 


(32) 


where  (•)  is  defined  in  Eq.  (30).  This  problem  can  be  reformulated  in  to  a  nonlinear  least  squares  problem 


where 


xn+i  =  arg  mm  4'T(xn+i)4'(xn+i) 

Xn+l 


^(Xn+l)  =  V/fXn+l) 


Sn+ 1 


Pn  0 
0  Rn+ 1 


V  (Xn+i) 


"0  (^7i7  ^n+1 1  i'n+l  ) 
zn+ 1 


Xn 

h{xn+ 1) 


(33) 


(34) 

(35) 

(36) 
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r  i  i  r  i_ 

and  S^+1  S*+1  =  5n+i.  In  addition  to  the  least  squares  formulation,  a  constraint  must  be  added  to 

preserve  the  quaternion  norm  ||q||  =  1.  A  number  of  methods  exist  for  iteratively  solving  this  problem.33 
For  this  research,  the  Levenberg-Marquardt  algorithm  was  implemented  due  to  its  robustness  and  global 
convergence  properties. 

Once  the  mode  has  been  found,  the  nonlinear  estimation  problem  is  essentially  solved.  However,  the  issue 
of  recursion  remains  unaddressed.  From  Eq.  (29),  the  obvious  difficulty  for  recursion  is  in  preserving  the 
inverse  flow  map  dependency  of  the  a  priori  pdf.  It  is  possible  to  preserve  this  dependency  completely  such 
that  the  solution  to  the  nonlinear  estimation  problem  at  any  future  time  t  >  to  would  require  a  backward 
propagation  to  the  initial  time  to-  This  approach  would  require  the  preservation  of  all  measurement  values 
up  to  and  including  zt.  However,  the  computational  expense  and  storage  burden  would  rapidly  accumulate, 
rendering  the  filter  inefficient  and  running  the  risk  of  placing  too  much  confidence  in  the  original  pdf,  which 
is  often  nothing  more  than  an  educated  guess.  Therefore,  to  ensure  an  efficient  algorithm,  the  iterative 
nonlinear  least  squares  method  described  above  is  coupled  with  linear  error  propagation  theory  which  has 
been  modified  for  Hamiltonian  systems. 


2.  Error  Covariance  Propagation 

The  deviation  between  a  deterministic  reference  trajectory  with  state  x  and  a  nonlinear  system  model 
with  state  x  can  be  described  by  a  linearized  stochastic  system2 


dt 


fix* 


_d_ 

cbq 


/(x*) 


Sxt 


(37) 


where  <5xt  €  Rm  =  xt  —  xt  is  state  deviation  at  time  t,  f(xt)  £  Rm  describes  the  deterministic  behavior  of 
the  system  state,  and  <5x0  ~  N  (xq  -  x0 ,  P0).  This  equation  can  be  discretized  as 


5xn+i  =  $(xn+i,x„)(5xn 


(38) 


where 


<F(xn+i,xn) 


dxn+i 

dx„ 


(39) 


$  €  KmXm  is  the  state  transition  matrix  used  in  linear  systems  theory  and  the  EKF.  Using  the  Darboux- 
Weinstein  theorem,34  it  can  be  shown  that  this  linearized  Hamiltonian  system  is  itself  Hamiltonian.  This 
motivates  the  application  of  symplectic  methods  to  the  linearized  equations  in  order  to  preserve  the  under¬ 
lying  geometric  structure  of  the  system.  Given  the  symplectic  flow  map  <f>'  for  the  satellite  attitude  problem 
described  in  Section  III, 

(40) 

S- 4>'{tn  +  2A t; X,  tn  +  At)  I  ...  -^-<j>'(tn+ 1 ;  x,  tn  +  (j  -  1) At)  I 

OX  »Xtn  +  At  vX  |Xtn  +  (j-l)At 

(41) 


(tn+1  ■>  Xni  ^n) 


dx 


</>'(tn  +  At;x,  tn) 


where  tn+ 1  =  tn  -f  jAt.  From  the  definition  of  a  flow  map  in  Eq.  (10),  it  is  clear  that 


3- 1 


dx 


0  (j'Tl+l  5^n,tn)  J  J_ 


dx, 


tn+(m+l)At 


771=0 


&Xt, 


-f-mAt 


dxn 


(42) 


This  describes  a  flow  map  which  is  also  symplectic  and  therefore  preserves  the  linearized  Hamiltonian  system 
in  Eq.  (38).  The  symplecticity  of  the  flow  map  can  be  demonstrated  straightforwardly  if 


Q 

(fn+1  j  xni  fn)  =  I 


(43) 


±i 


However, as  previously  discussed,  the  condition  |-g^ 
Thus,  Eq.  (42)  shows  that  -§^4>' {tn+\\ xn,  tn)  is  symplectic. 


ax , 


1  is  true  for  all  symplectic  numerical  methods. 
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Returning  to  the  definition  of  the  state  transition  matrix  in  Eq.  (39),  it  is  clear  that  $  preserves  the 
linearized  Hamiltonian  structure  of  Eq,  (38).  This  is  particularly  useful  since  the  components  of  $  are 
already  calculated  for  the  nonlinear  least  squares  method  to  determine  the  state  estimate.  Once  the  state 
estimate  xn+i  is  known  at  time  tn+i,  the  SF  propagates  the  error  covariance  matrix  estimate  Pn  from  time 
tn  using  standard  Kalman  Filter  equations 


Pn+i  =  $  (xn+i,xn)  Pn$T  (x„+i,xn) 


modified  such  that 


^  (^n+l )  ^n)  —  (fn+li^ni^n) 


(44) 


(45) 


where 


—  0  ( tn  ]  ^n+li  ^n-fl) 


(46) 


Note  that,  in  general,  the  state  estimate  xn+i  /  (j>'(tn+\\ xn,  fn),  and  therefore  the  notation  x„  has  been 
introduced  to  indicate  the  initial  state  at  time  tn  that  maps  to  the  state  estimate  xn+i  at  time  fn+i-  Once 
Pn+ 1  is  known,  it  is  used  to  approximate  the  a  priori  pdf  via  a  Gaussian  distribution.  Finally,  the  standard 
Kalman  Filter  equations  are  used  to  approximate  the  Gaussian  a  posteriori  pdf  and  its  covariance  matrix, 
Pn+l- 

This  approach  is  distinct  from  the  standard  EKF  in  that  it  exactly  conserves  the  underlying  geometry  of 
the  linearized  Hamiltonian  system.  Practically  speaking,  the  symplecticity  of  Hamiltonian  systems  implies 
the  preservation  of  the  volume  of  phase  space  subsets;  in  other  words,  the  volume  defined  by  a  set  of  system 
solutions  in  phase  space  at  any  given  time  will  remain  constant  for  all  time.  Therefore,  the  Hamiltonian 
nature  of  satellite  attitude  motion  demands  that  the  volumes  defined  by  the  level  sets  of  the  initial  state 
pdf  remain  constant  throughout  the  evolution  of  the  pdf.  For  a  deterministic  Hamiltonian  system  with  an 
initial  Gaussian  pdf  and  linearized  dynamics,  this  requirement  is  equivalent  to  preserving  the  volume  of  the 
ellipsoid  defined  by  the  error  covariance  matrix;  this  is  clearly  satisfied  by  the  state  transition  matrix  $  and 
its  unity  determinant. 


3.  Process  Noise 

The  final  unresolved  matter  for  SF  implementation  is  the  reintroduction  of  stochastic  accelerations  and 
process  noise.  In  the  standard  Kalman  Filter,  these  are  accounted  for  in  the  Q  matrix  which  is  generally 
assumed  to  be  additive  Gaussian  noise  with  covariance  matrix2 

/•At 

Qn+ 1=  /  $(*„+i,tn  +r)G(tn  +  T)Q{tn  +  r)GT(tn  +  r)$T  (£n+1,  +  r)dr  (47) 

Jo 

where  At  =  tn+i  —  tn,  G(tn  +  r)  relates  the  zero-mean  white  noise  process  defined  by  Q(tn  +r)  to  the  state, 
and  the  state  has  been  temporarily  omitted  from  the  notation  to  emphasize  dependency  on  time  (see  Eq. 
(17)  for  the  definitions  of  Q  and  G  in  the  dynamical  equations).  In  each  iteration  of  the  Kalman  Filter,  the 
process  noise  covariance  matrix  Qn+i  is  added  to  the  propagated  error  covariance  matrix  to  determine  the 
predicted  error  covariance 

P-n+l  =  $  (tn+l,tn)  Pn$T  (tn+l,tn)  +  Qn+ 1  (48) 

This  approach  will  generally  not  work  for  the  SF,  which  does  not  propagate  the  error  covariance  matrix 
until  after  the  state  estimate  has  been  determined.  Instead,  the  SF  solves  for  the  covariance  matrix  Qn  that, 
when  added  to  Pn,  leads  to  the  error  covariance  matrix  in  Eq.  (48): 

*1*  (tn- 1-1  >  tn)  [p„  +  Qnj  $T  (tn+ 1 ,  tn)  =  4?  (t„+ 1 ,  tn)  Pn<i>T  (tn+ 1  ,tn)  +  Qn+l  (49) 

This  readily  reduces  to 

Qn  =  $_1  0Cn+l>in)Qn+l($T)~1  (*„+!,*„)  (50) 
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Note  that  this  approach  assumes  the  state  transition  matrix  is  a  good  approximation  to  the  true  nonlinear 
flow.  This  is  justified  in  part  because  of  the  SF’s  theoretically  accurate  state  transition  matrix  $  (fn+j,  tn). 
Though  this  is  a  first-order  approximation,  it  is  nevertheless  acceptable  considering  that  process  noise  is 
treated  as  a  tuning  factor  in  filter  implementation,  therefore  requiring  less  rigorous  treatment. 

.  Since  $  is  known  to  a  high  degree  of  accuracy  in  the  SF,  a  solution  for  Qn  based  on  $  is  ideal.  Begin 
with  an  alternative  definition  of  <f>, 


where 


d<&  (t,  t) 
dt 


F(t)  = 


FWfrr) 


d_ 

<9x 


/(*) 


is  the  matrix  of  partial  derivatives  of  the  state  evaluated  at  t.  The  solution  is  a  matrix  exponential 

$(t,T)  =  e£F(s>ds 


(51) 

(52) 

(53) 


The  Taylor  expansion  of  the  matrix  exponential  gives 

$(t,T)  =  I  +  J  F(s)ds  +  H.O.T. 

Ignoring  higher  order  terms  (H.O.T)  and  applying  Euler’s  method  to  the  integral  gives 


(54) 


*(t,T)=I  +  F(t)(t-T) 

Assuming  stochastic  errors  in  the  angular  rates  only,  define  G  =  [03x4  I3]T  and 

/  ax  0  0 

Q  = 


0  02  0 

V  0  0  cr3 


such  that 


T  =  GQGt  = 


0  0 
0  Q 


where  T  €  E7x7.  Substituting  these  results  into  Eqn.  (47)  gives 

r/St 

Qn+ 1=  /  (I  +  E(tn+i)(tn+i  —  (tn  +  r))T(I  +  FT(tn+i)(tn+i  —  (tn  +  r))dr 

Jo 

This  integral  evaluates  to 

Qn+ 1  =  *A  t  +  T  FT(tn+1)^-  +  F(fn+1)T^  +  F(tn+1)?FT(tn+1)^- 


Rearranging  Eqn.  (55) 


F(tn+ 1)  = 


^(^n+lj^n)  I 

A t 


(55) 

(56) 

(57) 

(58) 

(59) 

(60) 


Substituting  this  definition  into  Eqn.  (59),  omitting  time  notation  for  convenience,  and  simplifying  gives 

Qn+i  =  4^  ($T  +  T$t  +  2$T$r  +  2T)  (61) 

D 

Finally,  applying  this  result  to  Eqn.  (50)  and  simplifying  gives 

Qn  =  ^(xr  +  rTxT  +  2T)  (62) 

where  x  =  I  +  ^_1  and  T  =  T (<I>'1)T.  When  applied  in  the  SF,  $  is  defined  by  Eq.  (45)  in  order  to  conserve 
the  linearized  Hamiltonian.  In  theory,  Qn  should  be  recalculated  at  every  iteration  of  the  Levenberg- 
Marquardt  algorithm.  However,  many  experiments  with  the  SF  have  shown  that  this  approach  leads  to 
negligible  performance  gain.  Therefore,  in  order  to  minimize  computational  expense,  Qn  is  calculated  once 
per  iteration. 
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C.  Summary 

The  nonlinear  Symplectic  Filter  is  summarized  below.  Assuming  an  initial  state  xn,  initial  error  covari¬ 
ance  matrix  Pn,  process  noise  matrix  Q,  and  system  observations  z  with  covariance  R,  the  SF  is  governed 
by  the  following  equations: 


Initial  Calculations 


T  = 

State  Prediction 

x„+i  = 
$(xn+i,x„)  = 
Xn  = 

r„  = 

Qn  = 

State  Estimation 

Pi+ 1  = 
P(xn+1)  = 

^(Xn+l)  = 
X„+l  = 

Error  Covariance  Propagation 

Xn  “ 
$(xn+l,xn)  = 

Hn+ 1  = 

Pn+l  = 
Pn+l  = 


0  0 
0  Q 


4*  (ifi+liXm^n) 

(^n+1  i  Xn,  fn) 

I  +  $_1(xn+i,xn) 

T($_1)T(xn+i,x„) 

f(x»Tn  +  t*£  +  TC) 


Pn  +  Qn  0 
0  Rn+ 1 

Xn 

/i(x„+i) 

S~ilV(Xn+l) 


P  (^m  Xn+1,  t«+l) 

zn+ 1 


arg  ^T(xn+1)^(xn+i) 

Xn+1 


Ip  (tn\  Xn+1  j  ^n-fl) 

^Xn+1 

dxn 

L  J  xn+i 

^  (x«n+1 ,  X„)  {Pn  +  (?„)4>T  (xtn+1 ,  X„) 

•Pn+1  —  Pn+l-^n+l^n+lPn+lPn+l  +  Pn+l]  1  Pn+lPn+1 


(63) 


(64) 

(65) 

(66) 

(67) 

(68) 


(69) 

(70) 

(71) 

(72) 

(73) 

(74) 

(75) 

(76) 

(77) 


To  illustrate  the  merits  of  the  SF  over  the  EKF,  consider  the  examples  in  figure  1 .  Given  a  nonlinear  pen¬ 
dulum,  assume  an  uncertainty  in  the  initial  conditions  in  phase  space  represented  by  a  Gaussian  distribution 
of  points  (blue).  At  some  future  time,  the  propagated  points  (red)  represent  the  a  priori  density.  The  figure 
on  the  top  right  is  the  EKF  approximation  which  uses  linearized  dynamics  and  only  calculates  the  first  two 
moments  of  the  a  priori  density  (represented  here  in  a  Gaussian  distribution).  The  figure  on  the  top  left  is 
the  SF  approximation  which  makes  no  assumption  about  the  form  of  the  a  priori  density  when  determining 
the  state  estimate  and  accurately  transforms  the  original  Gaussian  density  through  the  nonlinearity  of  the 
system  dynamics.  Here,  the  SF  approximates  the  a  priori  density  with  much  greater  fidelity  than  the  EKF. 
Assuming  that  these  probability  densities  are  based  on  a  predicted  state  that  is  slightly  perturbed  from  the 
true  state  and  incorporating  a  random  noisy  observation  gives  the  a  posteriori  densities  seen  in  the  lower 
left  and  right  figures.  The  SF’s  preservation  of  density  function  nonlinearity  allows  it  to  estimate  the  state 
to  a  higher  degree  of  accuracy  than  the  EKF. 
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V.  Results 

In  this  section,  a  number  of  performance  comparisons  are  made  between  the  SF  and  the  Iterated  Extended 
Kalman  Filter  (IEKF).2  The  IEKF  is  an  appropriate  benchmark  as  both  the  SF  and  IEKF  iteratively  solve 
for  the  mode  of  the  a  posteriori  pdf  as  the  system’s  state  estimate.  The  IEKF  uses  a  flight  tested,  non- 
symplectic,  second-order  Adams-Bashforth  integrator  combined  with  a  non-symplectic  second-order  state 
transition  matrix  algorithm,  making  it  equivalent  in  numerical  order  to  the  propagator  and  state  transition 
matrix  used  by  the  SF.  To  compare  the  two  estimation  methods,  realistic  satellite  configurations  and  mission 
profiles  were  selected  for  simulation.  A  representative  case  is  presented  here  based  on  the  upcoming  U.S.  Air 
Force  Academy  FalconSAT-3  small  satellite  mission.  This  is  modeled  as  a  50  kilogram,  nearly  axisymmetric 
satellite  in  a  560  km,  moderately  inclined,  circular  orbit  with  a  high-accuracy  observation  device  substituted 
for  the  standard  low-accuracy  magnetometer  planned  for  the  actual  mission.  Instead,  the  Advanced  Stellar 
Compass  (ASC)  is  modeled  due  to  its  successful  implementation  as  a  rate-gyro  replacement  onboard  an  agile 
resource-constrained  small  satellite.  It  has  the  ability  to  provide  frequent  high-accuracy  attitude  observations 
at  fast  rotation  rates.  Additionally,  it  outputs  quaternion  values,  thereby  eliminating  observation  nonlinearity 
from  quaternion-based  filters  and  leaving  the  dynamical  model  as  the  primary  source  of  nonlinearity  in  the 
estimation  process.  For  this  analysis,  ASC  sensor  noise  is  modeled  as  a  zero-mean  Gaussian  white-noise 
process  with  the  equivalent  of  one  arcsec  (lcr)  standard  deviation  in  each  axis.  A  symplectic  integrator  with 
a  small  time  step  relative  to  the  estimation  propagators  is  used  to  model  both  the  true  trajectory  and  the 
observation  set  using  the  ASC  properties  outlined  above. 
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For  the  first  simulation,  both  the  SF  and  the  IEKF  are  limited  to  one  iteration,  effectively  reducing 
the  IEKF  to  a  standard  EKF.  Both  filters  begin  with  exact  knowledge  of  angular  position  and  rate,  and 
the  satellite  body  begins  in  a  tumbling  state  with  an  angular  rate  vector  magnitude  of  7°s-1.  For  filter 
initialization,  the  diagonal  values  of  the  covariance  matrix  for  both  the  IEKF  and  SF  are  set  to  high  values 
relative  to  the  observation  noise  to  ensure  initial  convergence.  Observation  noise  and  process  noise  are  tuned 
in  each  filter  individually  to  give  optimal  performance.  A  standard  small  satellite  ADCS  step  size  of  0.1 
seconds  is  used  for  state  propagation,  and  observation  frequency  is  one  hertz. 

Figure  2  shows  the  norm  of  the  attitude  and  rate  errors  from  this  simulation.  The  IEKF  attitude  estimates 
converge  just  below  the  level  of  the  observation  noise.  High  process  noise  values  relative  to  observation  noise 
are  required  to  ensure  convergence,  inhibiting  the  IEKF’s  ability  to  filter  out  observation  noise  and  more 
accurately  estimate  the  state.  In  contrast,  the  SF  attitude  estimates  converge  with  a  standard  deviation 
that  is  roughly  an  order  of  magnitude  below  the  observation  noise  standard  deviation,  demonstrating  that 
the  SF  is  able  to  improve  upon  the  already  high-accuracy  observations  in  this  case.  Also,  the  SF  norm  rate 
errors  are  two  orders  of  magnitude  less  than  the  IEKF  norm  rate  errors,  demonstrating  the  ability  of  the 
SF  to  provide  high-accuracy  rate  estimation  given  well-understood  dynamics.  As  expected,  the  Jacobi-like 
integral  constant  is  conserved  to  a  high  degree  of  accuracy  as  well,  as  seen  in  figure  3. 


Figure  2.  Norm  of  Attitude  and  Rate  Errors:  Single  Iteration  and  No  Initial  State  Errors 


Figure  3.  Relative  Errors  of  Jacobi  Constant  for  Simulation  1 

To  determine  the  source  of  the  performance  difference  between  the  IEKF  and  the  SF,  the  SF  is  compared 
to  the  symplectic  EKF  (SKF)  in  Ref  17.  The  SF  is  unique  in  that  it  combines  a  symplectic-preserving 
dynamical  model  with  a  state  estimation  technique  that  fully  preserves  the  nonlinear  state  pdf;  in  contrast, 
the  SKF  combines  a  symplectic  dynamical  model  with  the  standard  EKF  algorithm.  Figure  4  illustrates 
that,  given  the  same  initial  conditions  as  the  first  simulation,  the  SKF  and  SF  give  nearly  identical  state 
estimation  results.  It  can  be  inferred  from  this  equivalence  that  the  symplectic  dynamical  model  is  the 
primary  source  of  performance  disparity  between  the  SF  and  the  IEKF  observed  in  figure  2.  By  using  a  non- 
symplectic  dynamical  model,  the  IEKF  introduces  nonlinear  errors  in  state  propagation  which  significantly 
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impairs  its  filtering  ability.  That  the  model  is  the  main  error  source  is  not  surprising  since  the  state  pdf 
is  unlikely  to  become  significantly  nonlinear  between  the  relatively  frequent  attitude  observations  modeled 
in  the  first  simulation.  Note  that  the  comparison  between  the  SKF  and  SF  has  the  additional  benefit  of 
validating  the  SF  algorithm  by  demonstrating  its  convergence  to  the  Kalman  Filter  in  quasi-linear  cases. 


Figure  4.  Equivalence  of  SKF  and  SF  State  Estimation 


The  next  simulation  demonstrates  the  strengths  of  the  nonlinear  SF  state  estimate  step.  In  this  case, 
initial  attitude  errors  of  60°,  30°,  and  30°  in  the  roll,  pitch,  and  yaw  axes,  respectively,  are  combined  with 
initial  rate  errors  of  3°s-1,  5°.s_1 ,  and  -4°s-1.  Initial  attitude  covariance  is  set  to  (4°)2  in  each  axis,  initial 
rate  covariance  is  set  to  (2°s-1)2  in  each  axis,  and  process  noise  is  equivalent  in  each  filter  and  low  relative 
to  the  observation  noise.  Additionally,  observation  noise  is  increased  to  100  arcseconds  (1<t).  This  represents 
a  worst-case  scenario  where  the  filter  is  confident  in  its  state  estimates  though  they  are  significantly  biased 
from  the  true  values.  Both  filters  achieve  optimal  performance  at  four  iterations  per  observation,  and  the 
results  are  presented  in  figures  5  and  6. 


Figure  5. 


Norm  of  Attitude  and  Rate  Errors:  Multiple  Iterations  and  Large  Initial  State  Errors 
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I 

1 


Time.  * 


Figure  6.  Relative  Errors  of  Jacobi  Constant  for  Simulation  2 


In  this  case,  The  SF  converges  much  faster  than  the  IEKF  to  the  true  state,  and  its  steady-state  accu¬ 
racies  are  orders  of  magnitude  better  than  the  IEFK’s.  The  higher  order  moments  of  the  a  posteriori  pdf 
preserved  by  the  SF  state  estimation  method  enable  it  to  more  accurately  filter  observation  noise,  whereas 
the  IEKF  preserves  only  the  first  two  moments  when  estimating  the  state  and  therefore  copes  poorly  with  the 
nonlinearity  induced  in  the  pdf  by  relatively  long  spans  between  observations.  By  the  end  of  the  simulation 
run  presented  above,  the  IEKF  has  not  yet  converged  to  the  level  of  the  observation  noise.  In  this  case, 
further  iterations  of  the  SF  and  the  IEKF  make  no  appreciable  difference  in  estimation  accuracy.  Clearly, 
the  SF’s  symplectic  dynamical  model  and  nonlinear  state  estimation  technique  enable  it  to  cope  with  system 
nonlinearity  better  than  the  IEKF. 


VI.  Conclusions 

In  this  paper  a  new  nonlinear  filter  for  Hamiltonian  systems,  called  the  Symplectic  Filter,  has  been  pre¬ 
sented.  It  is  unique  in  that  it  combines  a  symplectic  dynamical  model  with  a  nonlinear  estimation  process 
that  fully  preserves  the  probability  density  function  when  determining  the  system  state.  It  is  also  compu¬ 
tationally  efficient  as  it  propagates  error  covariances  using  Extended  Kalman  Filter  equations  which  have 
been  modified  to  preserve  the  underlying  geometry  of  Hamiltonian  systems.  By  combining  a  theoretically 
correct  approach  to  state  estimation  with  efficient  recursion  methods,  the  Symplectic  Filter  aims  to  provide 
accurate  estimation  for  small  satellites  with  limited  computational  resources. 

Comparisons  with  a  non-symplectic  second-order  Iterated  Extended  Kalman  Filter  demonstrate  the 
strengths  of  the  SF  when  applied  to  the  satellite  attitude  problem.  In  the  first  case,  the  symplectic  dy¬ 
namical  model  of  the  SF  enables  it  to  reduce  frequent  high-accuracy  attitude  observation  errors  by  an  order 
of  magnitude  while  the  IEKF  is  only  able  to  converge  to  the  observation  noise.  In  the  second  case,  the 
nonlinear  state  estimation  method  of  the  SF  enables  it  to  converge  to  the  true  solution  with  a  high  degree  of 
accuracy  given  large  initialization  errors.  These  results  demonstrate  the  merit  of  the  nonlinear  Symplectic 
Filter  and  its  suitability  for  use  onboard  agile  small  satellites  that  require  high  accuracy,  computationally 
efficient  attitude  estimation  methods. 
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